#include "stdafx.h"
#include "common.h"
#include "math.h"


void initVariables(node &Node) {
	unsigned int i;

	for (i=0; i<Node.demand_nums; i++) {
		Node.demand[i] = 0.5*(Node.demand_limit[i].up+Node.demand_limit[i].down);
	}

	for (i=0; i<Node.generator_nums; i++) {
		Node.generator[i] = 0.5*(Node.generator_limit[i].down+Node.generator_limit[i].up);
	}

	for (i=0; i<Node.oneHopNbr_nums; i++) {
		if (Node.oneHopNbr_flow_direction[i] == 1) {
			Node.oneHopNbr_flow[i] = 0.5*(Node.oneHopNbr_flow_limit[i].up
				                          +Node.oneHopNbr_flow_limit[i].down);
		}
		else {
			Node.oneHopNbr_flow[i] = 0;
		}
	}

	Node.stepsize = 0;
	Node.dualvar = 0;
}

//update the weight that used to estimate the dual variables
void updateDualWeight(node &Node) {
	unsigned int i;

	Node.dualvar_weight = 0;
	Node.dualvar_factor = 0;
	for (i=0; i<Node.oneHopNbr_nums; i++)
	{
		Node.oneHopNbr_dualvar_weight[i] = -pow(Node.oneHopNbr_flow_hassion[i],-1);
		Node.dualvar_weight += pow(Node.oneHopNbr_flow_hassion[i],-1);
		Node.dualvar_factor += pow(Node.oneHopNbr_flow_hassion[i],-1);
	}

	for (i=0; i<Node.demand_nums; i++) {
		Node.dualvar_weight += pow(Node.demand_hassion[i],-1);
	}
	for (i=0; i<Node.generator_nums; i++) {
		Node.dualvar_weight += pow(Node.generator_hassion[i],-1);
	}

    Node.dualvar_factor += fabs(Node.dualvar_weight);
	Node.dualvar_factor *= 0.51;
	Node.dualvar_weight -= Node.dualvar_factor;
}

void updateDualobservation(node &Node) {
	unsigned int i;
	Node.dualvar_observation = 0;

	for (i=0; i<Node.generator_nums; i++) {
		Node.dualvar_observation -= Node.generator[i]-pow(Node.generator_hassion[i],-1)*(Node.generator_gradient[i]-Node.dualvar);
	}

	for (i=0; i<Node.demand_nums; i++) {
		Node.dualvar_observation += Node.demand[i]-pow(Node.demand_hassion[i],-1)*(Node.demand_gradient[i]+Node.dualvar);
	}

	for (i=0; i<Node.oneHopNbr_nums; i++) {
		Node.dualvar_observation += Node.oneHopNbr_flow_direction[i]*(Node.oneHopNbr_flow[i]
			                        -pow(Node.oneHopNbr_flow_hassion[i],-1)*(Node.oneHopNbr_flow_gradient[i]+
									Node.oneHopNbr_flow_direction[i]*(Node.dualvar-Node.oneHopNbr_dualvar[i])));
	}
}

void updatePrimalDelta(node &Node) {
	unsigned int i;

	for (i=0; i<Node.generator_nums; i++) {
		Node.generator_delta[i] = -pow(Node.generator_hassion[i],-1)*(Node.generator_gradient[i]-(Node.dualvar+Node.dualvar_delta));
	}

	for (i=0; i<Node.demand_nums; i++) {
		Node.demand_delta[i] = -pow(Node.demand_hassion[i],-1)*(Node.demand_gradient[i]+(Node.dualvar+Node.dualvar_delta));
	}

	for (i=0; i<Node.oneHopNbr_nums; i++) {
		if (Node.oneHopNbr_flow_direction[i] == 1) {
			Node.oneHopNbr_flow_delta[i] = -pow(Node.oneHopNbr_flow_hassion[i],-1)*(Node.oneHopNbr_flow_gradient[i]
				+(Node.dualvar+Node.dualvar_delta-(Node.oneHopNbr_dualvar[i]+Node.oneHopNbr_dualvar_delta[i])));
		}
	}
}



void initResForm(node &Node) {
	unsigned int i;
	float tmp, updatedVal,updatedDualVal,updatedDualVal_NBR;

	updateGradient(Node);
	Node.form = 0;

	//nodal kcl&kvl
	tmp = 0;

	for (i=0; i<Node.generator_nums; i++) {
		updatedVal = Node.generator[i]+Node.stepsize*Node.generator_delta[i];
		if (updatedVal>=Node.generator_limit[i].up 
			||updatedVal<=Node.generator_limit[i].down) {
			Node.form = pow((Node.form_previous+3*CONSTANT_ETA),2);
			return;
		}
		tmp -= updatedVal;
	}

	for (i=0; i<Node.demand_nums; i++) {
		updatedVal = Node.demand[i]+Node.stepsize*Node.demand_delta[i];
		if (updatedVal>=Node.demand_limit[i].up 
			|| updatedVal<=Node.demand_limit[i].down) {
			Node.form = pow((Node.form_previous+3*CONSTANT_ETA),2);
			return;
		}
		tmp += updatedVal;
	}

	for (i=0; i<Node.oneHopNbr_nums; i++) {
		updatedVal = Node.oneHopNbr_flow[i]+Node.stepsize*Node.oneHopNbr_flow_delta[i];
		if (updatedVal >= Node.oneHopNbr_flow_limit[i].up
			|| updatedVal <= Node.oneHopNbr_flow_limit[i].down) {
				Node.form = pow((Node.form_previous+3*CONSTANT_ETA),2);
				return;
		}
		tmp += Node.oneHopNbr_flow_direction[i]*updatedVal;
	}

	Node.form += pow(tmp,2);

	//demand
	updatedDualVal = Node.dualvar+Node.stepsize*Node.dualvar_delta;
	for (i=0; i<Node.demand_nums; i++) {
		Node.form += pow((Node.demand_gradient[i]+updatedDualVal),2);
	}

	//generator
	for (i=0; i<Node.generator_nums; i++) {		
		Node.form += pow((Node.generator_gradient[i]-updatedDualVal),2);
	}

	//flow
	for (i=0; i<Node.oneHopNbr_nums; i++) {
		if (Node.oneHopNbr_flow_direction[i] == 1) {
			updatedDualVal_NBR = Node.oneHopNbr_dualvar[i]+Node.stepsize*Node.oneHopNbr_dualvar_delta[i];
			Node.form += pow((Node.oneHopNbr_flow_gradient[i]+
				(updatedDualVal-updatedDualVal_NBR)),2);
		}
	}
}

void updatePrimalVal(node &Node) {
	unsigned int i;

	for (i=0; i<Node.demand_nums; i++) {
		Node.demand[i] += Node.stepsize*Node.demand_delta[i];
	}

	for (i=0; i<Node.generator_nums; i++) {
		Node.generator[i] += Node.stepsize*Node.generator_delta[i];
	}

	for (i=0; i<Node.oneHopNbr_nums; i++) {
		if (Node.oneHopNbr_flow_direction[i] == 1) {
			Node.oneHopNbr_flow[i] += Node.stepsize*Node.oneHopNbr_flow_delta[i];
		}
	}

}

void updateDualVal(node &Node){
	Node.dualvar += Node.stepsize*Node.dualvar_delta;
}


